// Modified from osg/BoundingBox
#pragma once

#include <glm/glm.hpp>

namespace broadphase {

using scalar = float;
using vec3 = glm::tvec3<scalar>;

class AABB
{
public:
    /** Minimum extent. (Smallest X, Y, and Z values of all coordinates.) */
    vec3 _min;
    /** Maximum extent. (Greatest X, Y, and Z values of all coordinates.) */
    vec3 _max;

    /** Creates an uninitialized bounding box. */
    inline AABB() :
        _min(FLT_MAX, FLT_MAX, FLT_MAX),
        _max(-FLT_MAX, -FLT_MAX, -FLT_MAX) {}

    inline AABB(const AABB& bb) :
        _min(bb._min),
        _max(bb._max) {}

    /** Creates a bounding box initialized to the given extents. */
    inline AABB(
        scalar xmin, scalar ymin, scalar zmin,
        scalar xmax, scalar ymax, scalar zmax) :
        _min(xmin, ymin, zmin),
        _max(xmax, ymax, zmax) {}

    /** Creates a bounding box initialized to the given extents. */
    inline AABB(const vec3& min, const vec3& max) :
        _min(min),
        _max(max) {}

    /** Clear the bounding box. Erases existing minimum and maximum extents. */
    inline void init()
    {
        _min = vec3(FLT_MAX, FLT_MAX, FLT_MAX);
        _max = vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
    }

    inline bool operator == (const AABB& rhs) const { return _min == rhs._min && _max == rhs._max; }
    inline bool operator != (const AABB& rhs) const { return _min != rhs._min || _max != rhs._max; }

    /** Returns true if the bounding box extents are valid, false otherwise. */
    inline bool valid() const
    {
        return _max[0] >= _min[0] && _max[1] >= _min[1] && _max[2] >= _min[2];
    }

    /** Sets the bounding box extents. */
    inline void set(
        scalar xmin, scalar ymin, scalar zmin,
        scalar xmax, scalar ymax, scalar zmax)
    {
        _min = vec3(xmin, ymin, zmin);
        _max = vec3(xmax, ymax, zmax);
    }

    /** Sets the bounding box extents. */
    inline void set(const vec3& min_vec, const vec3& max_vec)
    {
        _min = min_vec;
        _max = max_vec;
    }

    inline scalar& xMin() { return _min[0]; }
    inline scalar xMin() const { return _min[0]; }

    inline scalar& yMin() { return _min[1]; }
    inline scalar yMin() const { return _min[1]; }

    inline scalar& zMin() { return _min[2]; }
    inline scalar zMin() const { return _min[2]; }

    inline scalar& xMax() { return _max[0]; }
    inline scalar xMax() const { return _max[0]; }

    inline scalar& yMax() { return _max[1]; }
    inline scalar yMax() const { return _max[1]; }

    inline scalar& zMax() { return _max[2]; }
    inline scalar zMax() const { return _max[2]; }

    /** Calculates and returns the bounding box center. */
    inline const vec3 center() const
    {
        return scalar(0.5f)*(_min + _max);
    }

    /** Calculates and returns the bounding box radius. */
    inline scalar radius() const
    {
        return glm::sqrt(radius2());
    }

    /** Calculates and returns the squared length of the bounding box radius.
    * Note, radius2() is faster to calculate than radius(). */
    inline scalar radius2() const
    {
        vec3 l = _max - _min;
        return scalar(0.25f)*(l[0] * l[0] + l[1] * l[1] + l[2] * l[2]);
    }

    /** Returns a specific corner of the bounding box.
    * pos specifies the corner as a number between 0 and 7.
    * Each bit selects an axis, X, Y, or Z from least- to
    * most-significant. Unset bits select the minimum value
    * for that axis, and set bits select the maximum. */
    inline const vec3 corner(unsigned int pos) const
    {
        return vec3(pos & 1 ? _max[0] : _min[0], pos & 2 ? _max[1] : _min[1], pos & 4 ? _max[2] : _min[2]);
    }

    /** Expands the bounding box to include the given coordinate.
    * If the box is uninitialized, set its min and max extents to v. */
    inline void expandBy(const vec3& v)
    {
        if (v[0]<_min[0]) _min[0] = v[0];
        if (v[0]>_max[0]) _max[0] = v[0];

        if (v[1]<_min[1]) _min[1] = v[1];
        if (v[1]>_max[1]) _max[1] = v[1];

        if (v[2]<_min[2]) _min[2] = v[2];
        if (v[2]>_max[2]) _max[2] = v[2];
    }

    /** Expands the bounding box to include the given coordinate.
    * If the box is uninitialized, set its min and max extents to
    * Vec3(x,y,z). */
    inline void expandBy(scalar x, scalar y, scalar z)
    {
        if (x<_min[0]) _min[0] = x;
        if (x>_max[0]) _max[0] = x;

        if (y<_min[1]) _min[1] = y;
        if (y>_max[1]) _max[1] = y;

        if (z<_min[2]) _min[2] = z;
        if (z>_max[2]) _max[2] = z;
    }

    /** Expands this bounding box to include the given bounding box.
    * If this box is uninitialized, set it equal to bb. */
    void expandBy(const AABB& bb)
    {
        if (!bb.valid()) return;

        if (bb._min[0]<_min[0]) _min[0] = bb._min[0];
        if (bb._max[0]>_max[0]) _max[0] = bb._max[0];

        if (bb._min[1]<_min[1]) _min[1] = bb._min[1];
        if (bb._max[1]>_max[1]) _max[1] = bb._max[1];

        if (bb._min[2]<_min[2]) _min[2] = bb._min[2];
        if (bb._max[2]>_max[2]) _max[2] = bb._max[2];
    }

    /** Returns the intersection of this bounding box and the specified bounding box. */
    AABB intersect(const AABB& bb) const
    {
        return AABB(
            (glm::max)(xMin(), bb.xMin()), (glm::max)(yMin(), bb.yMin()), (glm::max)(zMin(), bb.zMin()),
            (glm::min)(xMax(), bb.xMax()), (glm::min)(yMax(), bb.yMax()), (glm::min)(zMax(), bb.zMax()));
    }

    /** Return true if this bounding box intersects the specified bounding box. */
    bool overlap(const AABB& bb) const
    {
        return 
            (glm::max)(xMin(), bb.xMin()) <= (glm::min)(xMax(), bb.xMax()) &&
            (glm::max)(yMin(), bb.yMin()) <= (glm::min)(yMax(), bb.yMax()) &&
            (glm::max)(zMin(), bb.zMin()) <= (glm::min)(zMax(), bb.zMax());
    }

    /** Returns true if this bounding box contains the specified coordinate. */
    inline bool contains(const vec3& v) const
    {
        return valid() &&
            (v[0] >= _min[0] && v[0] <= _max[0]) &&
            (v[1] >= _min[1] && v[1] <= _max[1]) &&
            (v[2] >= _min[2] && v[2] <= _max[2]);
    }

    /** Returns true if this bounding box contains the specified coordinate allowing for specific epsilon. */
    inline bool contains(const vec3& v, scalar epsilon) const
    {
        return valid() &&
            ((v[0] + epsilon) >= _min[0] && (v[0] - epsilon) <= _max[0]) &&
            ((v[1] + epsilon) >= _min[1] && (v[1] - epsilon) <= _max[1]) &&
            ((v[2] + epsilon) >= _min[2] && (v[2] - epsilon) <= _max[2]);
    }

    inline scalar squaredExteriorDistance(const vec3& p) const
    {
        scalar dist2 = 0;
        for (int k = 0; k < 3; ++k)
        {
            if (_min[k] > p[k])
            {
                scalar aux = _min[k] - p[k];
                dist2 += aux * aux;
            }
            else if (p[k] > _max[k])
            {
                scalar aux = p[k] - _max[k];
                dist2 += aux * aux;
            }
        }
        return dist2;
    }

};

}
